#ifndef HEXAPOD_GAITGENERATOR
#define HEXAPOD_GAITGENERATOR

#include <stdbool.h>
#include <stdint.h>
#include "HexapodLeg.h"

typedef struct _GaitGenerator {
	float gaitHeight;
	float vxyGoal[2];
	float dYawGoal;
	float phase[LEG_CNT];
	float phasePast[LEG_CNT];
	uint8_t contact[LEG_CNT];
	
	Point3D startP[LEG_CNT];
	Point3D endP[LEG_CNT];
	Point3D idealP[LEG_CNT];
	Point3D pastP[LEG_CNT];
	
  bool firstRun;
} GaitGenerator;

void gaitGeneratorInit(GaitGenerator* gaitG);

void gaitSetGait(GaitGenerator* gaitG, float vxyGoalGlobal[2], float dYawGoal, float gaitHeight);
void gaitRestart(GaitGenerator* gaitG);
void gaitRun(GaitGenerator* gaitG, Point3D *feetPos);

void gaitGetFootPos(GaitGenerator* gaitG, uint8_t legID,Point3D *footPos);
// float* gaitGetFootVel(GaitGenerator* gaitG, uint8_t legID);

float gaitCycloidXYPosition(float startXY, float endXY, float phase);
// float gaitCycloidXYVelocity(float startXY, float endXY, float phase);
float gaitCycloidZPosition(float startZ, float height, float phase);
// float gaitCycloidZVelocity(float height, float phase);


#endif